#include <Arduino.h>
#include "Defines.h"
#include "DataStruct.h"
#include "../lib/RF24-1.3.11/RF24.h"
#include "../lib/TB6612/TB6612.h"

/**
 * 运动参数变量
 * */
//电机脉冲计数
volatile float Motor1Count = 0, Motor2Count = 0, Motor3Count = 0, Motor4Count = 0;
//实际速度单位cm/s
float Motor1ActualSpeed = 0, Motor2ActualSpeed = 0, Motor3ActualSpeed = 0, Motor4ActualSpeed = 0;
//目标实际速度单位cm/s
float TargetMotor1ActualSpeed = 0, TargetMotor2ActualSpeed = 0, TargetMotor3ActualSpeed = 0, TargetMotor4ActualSpeed = 0;
//目标速度PWM
u8 TargetMotor1Pwm = 0, TargetMotor2Pwm = 0, TargetMotor3Pwm = 0, TargetMotor4Pwm = 0;
/**
 * 数据通讯变量
 * */
Data_Package data; //无线数据
RF24 radio(48, 49);   // nRF24L01 (CE, CSN)
const byte address[6] = "00001";
unsigned long lastReceiveTime = 0;
unsigned long currentTime = 0;
/**
 * 电机初始化
 * */
TB6612 motor1(Motor1PwmPin, Motor1Pin1, Motor1Pin2);

TB6612 motor2(Motor2PwmPin, Motor2Pin1, Motor2Pin2);

TB6612 motor3(Motor3PwmPin, Motor3Pin1, Motor3Pin2);

TB6612 motor4(Motor4PwmPin, Motor4Pin1, Motor4Pin2);

TB6612 motor5(Motor5Pin1,Motor5Pin2);

TB6612 motor6(Motor6Pin1,Motor6Pin2);
/**************************************
 * Arduino初始化函数
 *
 *************************************/
void setup() {
    Serial.begin(9600);
//    重置数据
    resetData();
//    编码器端口初始化
    encoder_Init();
//    开启无线通讯
    radio.begin();
    radio.openReadingPipe(0, address);
    radio.setAutoAck(false);
    radio.setDataRate(RF24_250KBPS);
    radio.setPALevel(RF24_PA_LOW);
    radio.startListening();
}

void encoder_Init() {
    pinMode(Motor1CountAPin, INPUT);
    pinMode(Motor3CountAPin, INPUT);
    pinMode(Motor2CountAPin, INPUT);
    pinMode(Motor4CountAPin, INPUT);
}

/***************************************
 * Arduino主循环
 *
 ***************************************/
void loop() {
    //读取脉冲计算速度
    Read_Motor_V();
    // 接收数据
    if (radio.available()) {
        radio.read(&data, sizeof(Data_Package));
//        记录接收时间
        lastReceiveTime = millis();
    }
//    当前时间
    currentTime = millis();
    if (currentTime - lastReceiveTime > 1000) {
//        上次接收数据到当前时间超过一秒，认为遥控器下线，重置数据
        resetData();
    }
//    todo:在合适的时机计算各电机的PWM值（需要修改的地方）
    TargetMotor1Pwm = Incremental_Pi(Motor1ActualSpeed, TargetMotor1ActualSpeed);
    TargetMotor2Pwm = Incremental_Pi(Motor2ActualSpeed, TargetMotor2ActualSpeed);
    TargetMotor3Pwm = Incremental_Pi(Motor3ActualSpeed, TargetMotor3ActualSpeed);
    TargetMotor4Pwm = Incremental_Pi(Motor4ActualSpeed, TargetMotor4ActualSpeed);

//    printData();

//    这里使用区间判断的方式消除遥控器数值跳动
    if (data.j1PotX > 150) {
//        左
        moveSidewaysLeft();
    } else if (data.j1PotX < 100) {
//        右
        moveSidewaysRight();
    } else if (data.j1PotY > 160) {
//        前
        moveForward();
    } else if (data.j1PotY < 100) {
//        后
        moveBackward();
    } else if (data.j2PotX < 100 && data.j2PotY > 160) {
//        右前
        moveRightForward();
    } else if (data.j2PotX > 160 && data.j2PotY > 160) {
//        左前
        moveLeftForward();
    } else if (data.j2PotX < 100 && data.j2PotY < 100) {
//        右后
        moveRightBackward();
    } else if (data.j2PotX > 160 && data.j2PotY < 100) {
//        左后
        moveLeftBackward();
    } else if (data.j2PotX < 100) {
//        右转圈
        rotateRight();
    } else if (data.j2PotX > 150) {
//        左转圈
        rotateLeft();
    } else {
        stopMoving();
    }
//    上升或下降
    if (data.button4==1){
        motor5.forward();
    } else if(data.button3==1){
        motor5.backward();
    }
//    夹持或放置
    if (data.button2==1){
        motor6.forward();
    } else if (data.button1==1){
        motor6.backward();
    }
}


/*********************************************************************/

void Read_Motor_V() {
    unsigned long time = 0;
    Motor1Count = 0, Motor4Count = 0, Motor3Count = 0, Motor2Count = 0;
    time = millis() + 50;//读50毫秒
    attachInterrupt(digitalPinToInterrupt(Motor2CountAPin), addMotor2Count, RISING);
    attachInterrupt(digitalPinToInterrupt(Motor1CountAPin), addMotor1Count, RISING);
    attachInterrupt(digitalPinToInterrupt(Motor4CountAPin), addMotor4Count, RISING);
    attachInterrupt(digitalPinToInterrupt(Motor3CountAPin), addMotor3Count, RISING);
    while (millis() < time); //达到50毫秒关闭中断
    detachInterrupt(digitalPinToInterrupt(Motor2CountAPin));
    detachInterrupt(digitalPinToInterrupt(Motor1CountAPin));
    detachInterrupt(digitalPinToInterrupt(Motor4CountAPin));
    detachInterrupt(digitalPinToInterrupt(Motor3CountAPin));
    Motor2ActualSpeed = (float) (((Motor2Count / 1441) * 6.5 * PI) / 0.05);   //单位cm/s
    Motor1ActualSpeed = (float) (((Motor1Count / 1441) * 6.5 * PI) / 0.05);   //单位cm/s
    Motor4ActualSpeed = (float) (((Motor4Count / 1441) * 6.5 * PI) / 0.05);   //单位cm/s
    Motor3ActualSpeed = (float) (((Motor3Count / 1441) * 6.5 * PI) / 0.05);   //单位cm/s
}

u8 Incremental_Pi(float current_speed, float target_speed) {
    //静态变量存在程序全周期：pwm:增量输出,bias:本次偏差,last_bias:上次偏差,prev_bais_:上上次偏差
    static float pwm, bias, last_bias, prev_bias;
    //计算本次偏差e(k)
    bias = current_speed - target_speed;
    //增量式PID控制器
    pwm -= (kp * (bias - last_bias) + ki * bias + kd * (bias - 2 * last_bias + prev_bias));
    //保存上上次偏差
    prev_bias = last_bias;
    //保存上一次偏差
    last_bias = bias;
    //PWM 限幅度  PWM 最高为255  限制在250
    if (pwm < -250) {
        pwm = 250;
    }
    if (pwm > 250) {
        pwm = 250;
    }
//    Serial.println((u8) pwm);
    return (u8) pwm;         //增量输出
}


void Set_Pwm(int motor1PWM, int motor2PWM, int motor3PWM, int motor4PWM) {
//    todo:这里暂时写死了，需要动态设置电机的转速（需要修改的地方）
    motor1.setSpeed(255);
    motor2.setSpeed(255);
    motor3.setSpeed(255);
    motor4.setSpeed(255);
}

/*********************************************************************/

void addMotor1Count() {
    Motor1Count++;
}

void addMotor2Count() {
    Motor2Count++;
}

void addMotor4Count() {
    Motor4Count++;
}

void addMotor3Count() {
    Motor3Count++;
}

/*********************************************************************/

void moveForward() {
    Set_Pwm(TargetMotor1Pwm, TargetMotor2Pwm, TargetMotor3Pwm, TargetMotor4Pwm);
    motor1.forward();
    motor2.forward();
    motor3.forward();
    motor4.forward();
}

void moveBackward() {
    Set_Pwm(TargetMotor1Pwm, TargetMotor2Pwm, TargetMotor3Pwm, TargetMotor4Pwm);
    motor1.backward();
    motor2.backward();
    motor3.backward();
    motor4.backward();
}

void moveSidewaysRight() {
    Set_Pwm(TargetMotor1Pwm, TargetMotor2Pwm, TargetMotor3Pwm, TargetMotor4Pwm);
    motor1.backward();
    motor2.forward();
    motor3.backward();
    motor4.forward();
}

void moveSidewaysLeft() {
    Set_Pwm(TargetMotor1Pwm, TargetMotor2Pwm, TargetMotor3Pwm, TargetMotor4Pwm);
    motor1.forward();
    motor2.backward();
    motor3.forward();
    motor4.backward();
}

void rotateLeft() {
    Set_Pwm(TargetMotor1Pwm, TargetMotor2Pwm, TargetMotor3Pwm, TargetMotor4Pwm);
    motor3.backward();
    motor4.backward();
    motor1.forward();
    motor2.forward();
}

void rotateRight() {
    Set_Pwm(TargetMotor1Pwm, TargetMotor2Pwm, TargetMotor3Pwm, TargetMotor4Pwm);
    motor3.forward();
    motor4.forward();
    motor1.backward();
    motor2.backward();
}

void moveRightForward() {
    Set_Pwm(0, TargetMotor2Pwm, TargetMotor3Pwm, 0);
    motor3.forward();
    motor2.forward();
    motor1.stop();
    motor4.stop();
}

void moveRightBackward() {
    Set_Pwm(TargetMotor1Pwm, 0, 0, TargetMotor4Pwm);
    motor1.backward();
    motor4.backward();
    motor3.stop();
    motor2.stop();
}

void moveLeftForward() {
    Set_Pwm(TargetMotor1Pwm, 0, 0, TargetMotor4Pwm);
    motor1.forward();
    motor4.forward();
    motor2.stop();
    motor3.stop();
}

void moveLeftBackward() {
    Set_Pwm(0, TargetMotor2Pwm, TargetMotor3Pwm, 0);
    motor2.backward();
    motor3.backward();
    motor1.stop();
    motor4.stop();
}

void stopMoving() {
    Set_Pwm(0, 0, 0, 0);
    motor1.stop();
    motor2.stop();
    motor3.stop();
    motor4.stop();
}

/*********************************************************************/

void resetData() {
    // Reset the values when there is no radio connection - Set initial default values
    data.j1PotX = 127;
    data.j1PotY = 127;
    data.j2PotX = 127;
    data.j2PotY = 127;
    data.j1Button = 1;
    data.j2Button = 1;
    data.pot1 = 1;
    data.pot2 = 1;
    data.tSwitch1 = 1;
    data.tSwitch2 = 1;
    data.button1 = 1;
    data.button2 = 1;
    data.button3 = 1;
    data.button4 = 1;
}

/*********************************************************************/

void printData() {

    Serial.print("; Motor1ActualSpeed");
    Serial.print(Motor1ActualSpeed);
    Serial.print("; Motor2ActualSpeed");
    Serial.print(Motor2ActualSpeed);
    Serial.print("; Motor3ActualSpeed");
    Serial.print(Motor3ActualSpeed);
    Serial.print("; Motor4ActualSpeed");
    Serial.print(Motor4ActualSpeed);

    // Print the data in the Serial Monitor
    Serial.print("; button1: ");
    Serial.print(data.button1);
    Serial.print("; button2: ");
    Serial.print(data.button2);
    Serial.print("; button3: ");
    Serial.print(data.button3);
    Serial.print("; button4: ");
    Serial.print(data.button4);
    Serial.print("; j1PotX: ");
    Serial.print(data.j1PotX);
    Serial.print("; j1PotY: ");
    Serial.print(data.j1PotY);
    Serial.print("; j2PotX: ");
    Serial.print(data.j2PotX);
    Serial.print("; j2PotY: ");
    Serial.print(data.j2PotY);
    Serial.print("; tSwitch1: ");
    Serial.print(data.tSwitch1);
    Serial.print("; tSwitch2: ");
    Serial.println(data.tSwitch2);
}